#include "mbot_linux_serial.h"
#include <serial/serial.h>
#include <iostream>

serial::Serial sp;
const unsigned char ender[2] = {0x0d, 0x0a};
const unsigned char header[2] = {0x55, 0xaa};

union sendData
{
    short d;
    unsigned char data[2];
};
union sendData angle[7];

union receiveData
{
    short d;
    unsigned char data[2];
} leftVelNow, rightVelNow, angleNow;

void Serial_Init()
{
    serial::Timeout to = serial::Timeout::simpleTimeout(100);
    serial::parity_t pt = serial::parity_t::parity_none;
    serial::bytesize_t bt = serial::bytesize_t::eightbits;
    serial::flowcontrol_t ft = serial::flowcontrol_t::flowcontrol_none;
    serial::stopbits_t st = serial::stopbits_t::stopbits_one;

    sp.setPort("/dev/ttyTHS0");
    sp.setBaudrate(57600);
    sp.setParity(pt);
    sp.setBytesize(bt);
    sp.setFlowcontrol(ft);
    sp.setStopbits(st);

    sp.setTimeout(to);

    try
    {
        sp.open();
    }
    catch (serial::IOException &e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return;
    }
}

void writeSpeed(int *p, unsigned char ctrlFlag)
{
    unsigned char buf[20] = {0};
    int i, length = 0;

    for (int i = 0; i < 7; i++)
    {
        angle[i].d = p[i];
    }

    for (i = 0; i < 2; i++)
        buf[i] = header[i];

    length = 13;
    buf[2] = length;
    for (i = 0; i < 2; i++)
    {
        buf[i + 3] = angle[0].data[i];
        buf[i + 5] = angle[1].data[i];
        buf[i + 7] = angle[2].data[i];
        buf[i + 9] = angle[3].data[i];
        buf[i + 11] = angle[4].data[i];
        buf[i + 13] = angle[5].data[i];
    }

    buf[3 + length - 1] = ctrlFlag;

    buf[3 + length] = getCrc8(buf, 3 + length);
    buf[3 + length + 1] = ender[0];
    buf[3 + length + 2] = ender[1];

    sp.write(buf, 20);
}

bool readSpeed(double &Left_v, double &Right_v, double &Angle, unsigned char &ctrlFlag)
{
    char i, length = 0;
    unsigned char checkSum;
    unsigned char buf[150] = {0};
    int n;

    size_t Receive_N = sp.available();
    if (Receive_N != 0)
    {
        n = sp.read(buf, Receive_N);

        if (buf[0] != header[0] || buf[1] != header[1])
        {
            ROS_ERROR("Received message header error!");
            return false;
        }

        length = buf[2];

        checkSum = getCrc8(buf, 3 + length);
        if (checkSum != buf[3 + length])
        {
            ROS_ERROR("Received data check sum error!");
            return false;
        }

        for (i = 0; i < 2; i++)
        {
            leftVelNow.data[i] = buf[i + 3];
            rightVelNow.data[i] = buf[i + 5];
            angleNow.data[i] = buf[i + 7];
        }

        ctrlFlag = buf[9];

        Left_v = leftVelNow.d;
        Right_v = rightVelNow.d;
        Angle = angleNow.d;

        return true;
    }
}

unsigned char getCrc8(unsigned char *ptr, unsigned short len)
{
    unsigned char crc;
    unsigned char i;
    crc = 0;
    while (len--)
    {
        crc ^= *ptr++;
        for (i = 0; i < 8; i++)
        {
            if (crc & 0x01)
                crc = (crc >> 1) ^ 0x8C;
            else
                crc >>= 1;
        }
    }
    return crc;
}
